#include "depth2pointcloud.h"
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <pcl_ros/transforms.h>


Depth2PointCloud::Depth2PointCloud(){
    init_camera_info_ = false;
    init_params_ = false;
}


// 设置图像数据
void Depth2PointCloud::setCameraInfo(sensor_msgs::CameraInfo::Ptr& camera_info_ptr){
    camera_info_ptr_ = std::make_shared<sensor_msgs::CameraInfo>(*camera_info_ptr);

    // 计算转点云缓存矩阵
    if(PoseMatX_.isZero() || PoseMatY_.isZero()){
        // cout << "PoseMatX_.isZero: " << (PoseMatX_.isZero() ? "true" : "false") 
        //     << "PoseMatY_.isZero: " << (PoseMatY_.isZero() ? "true" : "false") << endl;
        float cx = camera_info_ptr_->K[2];
        float cy = camera_info_ptr_->K[5];
        float fx = camera_info_ptr_->K[0];
        float fy = camera_info_ptr_->K[4];    
        PoseMatX_.resize(camera_info_ptr_->height,camera_info_ptr_->width);
        PoseMatY_ = PoseMatX_;
        for(int h = 0; h < camera_info_ptr_->height; h++){
            for (int w = 0; w < camera_info_ptr_->width; w++){
                PoseMatX_(h,w) = -(w - cx) / fx;
                PoseMatY_(h,w) = -(h - cy) / fy;
            }
        }    
    }
    init_camera_info_ = true;
}

// 设置传感器参数数据
void Depth2PointCloud::setParams(const SensorParams& sensor_params){
    sensor_params_ = sensor_params;
    init_params_ = true;
}

// 获取点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr Depth2PointCloud::getPointCloud(sensor_msgs::Image::Ptr depth_image_ptr){
    if(!camera_info_ptr_){
        LOG(WARNING) << "camera_info_ptr_ is null";
        return nullptr;
    }

    // 获取图像数据
    Eigen::MatrixXd depthmat;
	try{
		cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(depth_image_ptr, sensor_msgs::image_encodings::TYPE_32FC1);
		// cv::Mat depthImage = cv_ptr->image;
        cv::cv2eigen(cv_ptr->image, depthmat);
	} 
	catch(cv_bridge::Exception &e){
        LOG(ERROR) << "cv_bridge exception: " << e.what();
		return nullptr;
	}

    // 深度图转点云
    Eigen::MatrixXd depthmat_x,depthmat_y,depthmat_z;
    depthmat_x = depthmat * 0.001;
    depthmat_y = depthmat_x.array() * PoseMatX_.array();
    depthmat_z = depthmat_x.array() * PoseMatY_.array();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw(new pcl::PointCloud<pcl::PointXYZ>());
    for(int w=0; w<depthmat.cols(); w+=2){        
        for(int h=0; h<depthmat.rows(); h+=2){
            if(depthmat_x(h,w) == 0.0 && depthmat_y(h,w) == 0.0){  //原点数据不参与处理
                // cout << "x: " << depthmat_y(h,w) << ", y: " << depthmat_y(h,w) << endl;
                ;
            }
            else{
                // 填充点云
                pcl::PointXYZ p(depthmat_x(h,w), depthmat_y(h,w), depthmat_z(h,w));
                cloud_raw->points.emplace_back(p);
            }
        }   
    }
    cloud_raw->header.frame_id = depth_image_ptr->header.frame_id;
    cloud_raw->height = 1;
    cloud_raw->is_dense = false;
    cloud_raw->width = cloud_raw->points.size();

    // 获取变换信息
    Eigen::Translation3d translation(sensor_params_.transform.x, sensor_params_.transform.y, sensor_params_.transform.z);
    Eigen::Matrix3d R = 
        Eigen::AngleAxisd(sensor_params_.transform.yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix() *
        Eigen::AngleAxisd(sensor_params_.transform.pitch, Eigen::Vector3d::UnitY()).toRotationMatrix() *
        Eigen::AngleAxisd(sensor_params_.transform.roll, Eigen::Vector3d::UnitX()).toRotationMatrix();
    Eigen::Matrix4d transformMatrix = Eigen::Matrix4d::Identity();
    transformMatrix.block<3, 3>(0, 0) = R;
    transformMatrix.block<3, 1>(0, 3) = translation.vector();
    Eigen::Matrix4d inverseMatrix = transformMatrix.inverse();
    
    // 点云数据转换到base_link坐标系
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*cloud_raw, *cloud_base_link, transformMatrix);

    return cloud_base_link;
}